#include <rtthread.h>

#define DBG_TAG "mpu6050"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#include "mpu6xxx.h"
#include "mpu6050.h"
#include <math.h>

#define IMU_DELTA_T 0.005f /* 5ms计算一次 */
#define IMU_M_PI 3.1425f

#define IMU_NEW_WEIGHT 0.35f /* 新值权重 */
#define IMU_OLD_WEIGHT 0.65f /* 旧值权重 */

float g_param_kp = 50.0f; /* 加速度计(磁力计)的收敛速率比例增益50 */
float g_param_ki = 0.20f; /* 陀螺仪收敛速率的积分增益 0.2 */

struct mpu6xxx_device *mpu6050_dev;

quater_info_t g_q_info = {1, 0, 0}; /* 全局四元数 */

/**
 * @brief       开方函数
 * @param       x : 待开方的值
 * @retval      开方结果
 */
static float mpu_inv_sqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long *)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(float *)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

/**
 * @brief       数据转换
 * @note        对加速度数据做一阶低通滤波(参考匿名)，对gyro转成弧度每秒(2000dps)
 * @param       gx, gy, gz  : 3轴陀螺仪数据指针
 * @param       ax, ay, az  : 3轴加速度数据指针
 * @retval      无
 */
static void mpu_data_transform(float *gx, float *gy, float *gz, float *ax, float *ay, float *az)
{
    static double lastax = 0;
    static double lastay = 0;
    static double lastaz = 0;

    *ax = *ax * IMU_NEW_WEIGHT + lastax * IMU_OLD_WEIGHT;
    *ay = *ay * IMU_NEW_WEIGHT + lastay * IMU_OLD_WEIGHT;
    *az = *az * IMU_NEW_WEIGHT + lastaz * IMU_OLD_WEIGHT;

    lastax = *ax;
    lastay = *ay;
    lastaz = *az;

    *gx = *gx * IMU_M_PI / 180 / 16.4f;
    *gy = *gy * IMU_M_PI / 180 / 16.4f;
    *gz = *gz * IMU_M_PI / 180 / 16.4f;
}

/**
 * @brief       姿态解算融合, 核心算法
 * @note        使用的是互补滤波算法，没有使用Kalman滤波算法
 *              尽量保证该函数的调用频率为: IMU_DELTA_T , 否则YAW会相应的偏大/偏小
 * @param       gx, gy, gz  : 3轴陀螺仪数据
 * @param       ax, ay, az  : 3轴加速度数据
 * @retval      无
 */
static void mpu_ahrsupdate_nomagnetic(float gx, float gy, float gz, float ax, float ay, float az)
{
    static float i_ex, i_ey, i_ez; /* 误差积分 */

    float half_t = 0.25f * IMU_DELTA_T;
    float vx, vy, vz; /* 当前的机体坐标系上的重力单位向量 */
    float ex, ey, ez; /* 四元数计算值与加速度计测量值的误差 */
    float q0 = g_q_info.q0;
    float q1 = g_q_info.q1;
    float q2 = g_q_info.q2;
    float q3 = g_q_info.q3;
    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;
    float delta_2 = 0;

    /* 对加速度数据进行归一化 得到单位加速度 */
    float norm = mpu_inv_sqrt(ax * ax + ay * ay + az * az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    ex = ay * vz - az * vy;
    ey = az * vx - ax * vz;
    ez = ax * vy - ay * vx;

    /* 用差乘误差来做PI修正陀螺仪零偏，
     * 通过调节 g_param_kp，g_param_ki 两个参数，
     * 可以控制加速度计修正陀螺仪积分姿态的速度。*/
    i_ex += IMU_DELTA_T * ex; /* integral error scaled by Ki */
    i_ey += IMU_DELTA_T * ey;
    i_ez += IMU_DELTA_T * ez;

    gx = gx + g_param_kp * ex + g_param_ki * i_ex;
    gy = gy + g_param_kp * ey + g_param_ki * i_ey;
    gz = gz + g_param_kp * ez + g_param_ki * i_ez;

    /*数据修正完成，下面是四元数微分方程*/

    /* 四元数微分方程，其中half_t为测量周期的1/4，gx gy gz为陀螺仪角速度，
       以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程 */
    //    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * half_t;
    //    q1 = q1 + ( q0 * gx + q2 * gz - q3 * gy) * half_t;
    //    q2 = q2 + ( q0 * gy - q1 * gz + q3 * gx) * half_t;
    //    q3 = q3 + ( q0 * gz + q1 * gy - q2 * gx) * half_t;
    delta_2 = (2 * half_t * gx) * (2 * half_t * gx) + (2 * half_t * gy) * (2 * half_t * gy) + (2 * half_t * gz) * (2 * half_t * gz);

    /* 整合四元数率    四元数微分方程  四元数更新算法，二阶毕卡法 */
    q0 = (1 - delta_2 / 8) * q0 + (-q1 * gx - q2 * gy - q3 * gz) * half_t;
    q1 = (1 - delta_2 / 8) * q1 + (q0 * gx + q2 * gz - q3 * gy) * half_t;
    q2 = (1 - delta_2 / 8) * q2 + (q0 * gy - q1 * gz + q3 * gx) * half_t;
    q3 = (1 - delta_2 / 8) * q3 + (q0 * gz + q1 * gy - q2 * gx) * half_t;

    /* normalise quaternion */
    norm = mpu_inv_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    g_q_info.q0 = q0 * norm;
    g_q_info.q1 = q1 * norm;
    g_q_info.q2 = q2 * norm;
    g_q_info.q3 = q3 * norm;
}

void mpu_separate_gravity(float rx, float ry, float rz)
{
    float q0 = g_q_info.q0;
    float q1 = g_q_info.q1;
    float q2 = g_q_info.q2;
    float q3 = g_q_info.q3;
    float out_ax;
    float out_ay;
    float out_az;

    // 重力向量在全局坐标系中的表示
    float gravity[3] = {0, 0, 9.81};

    // 使用四元数旋转矩阵将重力向量从全局坐标系转换到设备坐标系
    float rotated_gravity[3];
    rotated_gravity[0] = 2 * (q1 * q3 - q0 * q2) + gravity[0] * (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) +
                         2 * gravity[1] * (q1 * q0 + q2 * q3) + 2 * gravity[2] * (q2 * q0 - q1 * q3);
    rotated_gravity[1] = 2 * (q2 * q3 + q0 * q1) + gravity[0] * (q1 * q0 - q2 * q3) +
                         2 * gravity[1] * (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) +
                         2 * gravity[2] * (q3 * q0 + q1 * q2);
    rotated_gravity[2] = 2 * (q1 * q2 - q0 * q3) + gravity[0] * (q2 * q0 + q1 * q3) +
                         2 * gravity[1] * (q3 * q0 - q1 * q2) +
                         2 * gravity[2] * (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);

    // 从加速度计读数中减去重力分量，得到动态加速度
    out_ax = rx - rotated_gravity[0];
    out_ay = ry - rotated_gravity[1];
    out_az = rz - rotated_gravity[2];
    printf(" gx %f,  gy %f,  gz %f\r", out_ax, out_ay, out_az);
}

/**
 * @brief       得到姿态解算后的欧拉角
 * @param       gx, gy, gz  : 3轴陀螺仪数据
 * @param       ax, ay, az  : 3轴加速度数据
 * @retval      返回值 : 欧拉角
 */
void mpu_get_gravity(float gx, float gy, float gz, float ax, float ay, float az, float rx, float ry, float rz)
{

    mpu_data_transform(&gx, &gy, &gz, &ax, &ay, &az); /* 数据转换 */
    // printf(" gx %f,  gy%f,  gz%f,  ax%f,  ay%f,  az%f\r", gx, gy, gz, ax, ay, az);
    mpu_ahrsupdate_nomagnetic(gx, gy, gz, ax, ay, az); /* 姿态解算 */
    // printf(" gx %f,  gy%f,  gz%f,  ax%f,  ay%f,  az%f\r", gx, gy, gz, ax, ay, az);

    mpu_separate_gravity(rx, ry, rz);
}

/**
 * @brief       得到姿态解算后的欧拉角
 * @param       gx, gy, gz  : 3轴陀螺仪数据
 * @param       ax, ay, az  : 3轴加速度数据
 * @retval      返回值 : 欧拉角
 */
eulerian_angles_t mpu_get_eulerian_angles(float gx, float gy, float gz, float ax, float ay, float az)
{
    eulerian_angles_t eulerangle;

    mpu_data_transform(&gx, &gy, &gz, &ax, &ay, &az); /* 数据转换 */
    // printf(" gx %f,  gy%f,  gz%f,  ax%f,  ay%f,  az%f\r", gx, gy, gz, ax, ay, az);
    mpu_ahrsupdate_nomagnetic(gx, gy, gz, ax, ay, az); /* 姿态解算 */
    // printf(" gx %f,  gy%f,  gz%f,  ax%f,  ay%f,  az%f\r", gx, gy, gz, ax, ay, az);

    float q0 = g_q_info.q0;
    float q1 = g_q_info.q1;
    float q2 = g_q_info.q2;
    float q3 = g_q_info.q3;

    eulerangle.pitch = -asin(-2 * q1 * q3 + 2 * q0 * q2) * 190 / IMU_M_PI;
    eulerangle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / IMU_M_PI;
    eulerangle.yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / IMU_M_PI;

    // printf("\r\nPITCH:%f\r\n", eulerangle.pitch);
    // printf("ROLL:%f\r\n", eulerangle.roll);
    // printf("YAW:%f\r\n", eulerangle.yaw);

    /* 可以不用作姿态限度的限制 */
    if (eulerangle.roll > 90 || eulerangle.roll < -90)
    {
        if (eulerangle.pitch > 0)
        {
            eulerangle.pitch = 180 - eulerangle.pitch;
        }

        if (eulerangle.pitch < 0)
        {
            eulerangle.pitch = -(180 + eulerangle.pitch);
        }
    }

    if (eulerangle.yaw > 180)
    {
        eulerangle.yaw -= 360;
    }
    else if (eulerangle.yaw < -180)
    {
        eulerangle.yaw += 360;
    }

    return eulerangle;
}

uint8_t mpu6050_init()
{
    /* Initialize mpu6xxx, The parameter is RT_NULL, means auto probing for i2c*/
    mpu6050_dev = mpu6xxx_init(MPU6XXX_DEVICE_NAME, RT_NULL);

    if (mpu6050_dev == RT_NULL)
    {
        rt_kprintf("mpu6xxx init failed\n");
        return -1;
    }
    rt_kprintf("mpu6xxx init succeed\n");
}

rt_err_t mpu6050_get_data(struct mpu6xxx_3axes *accel, struct mpu6xxx_3axes *gyro)
{

    if (mpu6xxx_get_accel(mpu6050_dev, &accel) != RT_EOK)
    {
        rt_kprintf("mpu6xxx get accel failed\n");
        return -1;
    }
    if (mpu6xxx_get_gyro(mpu6050_dev, &gyro) != RT_EOK)
    {
        rt_kprintf("mpu6xxx get gyro failed\n");
        return -1;
    }

    return RT_EOK;
}
